/*
 * topological-hex, a program to compute combinatorial hexahedral meshes.
 *
 * Copyright (C) <2018> <Université catholique de Louvain (UCL), Belgique>
 *
 * List of the contributors to the development, description and complete
 * License: see LICENSE file.
 *
 * This program (topological-hex) is free software:
 * you can redistribute it and/or modify it under the terms
 * of the GNU General Public License as published by the Free
 * Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program (see COPYING file).  If not,
 * see <http://www.gnu.org/licenses/>.
 */

#include "homology.h"

#include <stdint.h>
#include <string.h>
#include <stdio.h>

/**
 * All functions below assume that matrices are stored in a colum-major order,
 * and that column are zero-padded up to 256-bits.
 */

static
size_t find_pivot(uint64_t * restrict matrix, size_t n, size_t m,
                  size_t i, size_t *first_row);

static void swap_columns(uint64_t * restrict matrix, size_t n, size_t m,
                         size_t i, size_t j);

static void add_column(uint64_t * restrict matrix, size_t n, size_t m,
                       size_t dst_i, size_t src_i);

size_t nullspace(uint64_t * restrict matrix,
                 uint64_t * restrict kernel, size_t n, size_t m) {
  /* size_t n_words = ((n + 63) & ~63ul) / 64; */
  /* size_t m_words = ((m + 63) & ~63ul) / 64; */
  size_t m_words = 4, n_words = 4;

  for (size_t i = 0; i < m_words * m; i++)
    kernel[i] = 0;

  for (size_t i = 0; i < m; i++)
    kernel[(i / 64) + m_words * i] = 1ul << (i % 64);

  size_t i = 0;
  size_t pivot_i, first_row = 0;
  while ((pivot_i = find_pivot(matrix, n, m, i, &first_row)) != m) {
    swap_columns(matrix, n, m, i, pivot_i);
    swap_columns(kernel, m, m, i, pivot_i);

    for (size_t col_i = 0; col_i < m; col_i++) {
      if (col_i == i) continue;
      if (matrix[(first_row / 64) + n_words * col_i] & (1ul << (first_row % 64))) {
        add_column(matrix, n, m, col_i, i);
        add_column(kernel, m, m, col_i, i);
      }
    }
    i++;
    first_row++;
  }

  return m - i;
}

static uint64_t find_first_bit(uint64_t * restrict row, size_t n);

static
size_t find_pivot(uint64_t * restrict matrix, size_t n, size_t m,
                  size_t i, size_t *first_row) {
  /* size_t n_words = ((n + 63) & ~63ul) / 64; */
  size_t n_words = 4;

  size_t best_i = m;
  size_t best_row = n;
  for (size_t j = i; j < m && best_row != *first_row; j++) {
    size_t row_id = find_first_bit(matrix + n_words * j, n);
    if (row_id < best_row) {
      best_row = row_id;
      best_i = j;
    }
  }

  *first_row = best_row;
  return best_i;
}

static uint64_t find_first_bit(uint64_t * restrict row, size_t n) {
  /* size_t n_words = ((n + 63) & ~63ul) / 64; */
  size_t n_words = 4;

  row = ASSUME_ALIGNED(row);

  size_t i = 0;
  for (i = 0; i < n_words && row[i] == 0; i++);
  if (i == n_words) return n;

  i = 64 * i + (row[i] == 0 ? 64 : find_lsb64(row[i]));
  return i >= n ? n : i;
}

static void swap_columns(uint64_t * restrict matrix, size_t n, size_t m,
                         size_t i, size_t j) {
  matrix = ASSUME_ALIGNED(matrix);

  ALIGNED uint64_t * restrict p_i = matrix + i * 4;
  ALIGNED uint64_t * restrict p_j = matrix + j * 4;
  ALIGNED uint64_t col_i[4], col_j[4];

  for (int i = 0; i < 4; i++) col_i[i] = p_i[i];
  for (int i = 0; i < 4; i++) col_j[i] = p_j[i];

  for (int i = 0; i < 4; i++) p_i[i] = col_j[i];
  for (int i = 0; i < 4; i++) p_j[i] = col_i[i];
}

static void add_column(uint64_t * restrict matrix, size_t n, size_t m,
                       size_t dst_i, size_t src_i) {
  matrix = ASSUME_ALIGNED(matrix);

  ALIGNED uint64_t * restrict p_dst = matrix + dst_i * 4;
  ALIGNED uint64_t * restrict p_src = matrix + src_i * 4;

  ALIGNED uint64_t src[4];
  ALIGNED uint64_t dst[4];

  for (int i = 0; i < 4; i++) src[i] = p_src[i];
  for (int i = 0; i < 4; i++) dst[i] = p_dst[i];
  for (int word_i = 0; word_i < 4; word_i++)
    p_dst[word_i] = dst[word_i] ^ src[word_i];
}

static void add_vectors(uint64_t * restrict v_dst,
                        const uint64_t * restrict v_src, size_t n) {
  v_dst = ASSUME_ALIGNED(v_dst);
  v_src = ASSUME_ALIGNED(v_src);

  for (int word_i = 0; word_i < 4; word_i++)
    v_dst[word_i] ^= v_src[word_i];
}

static bool eliminate(uint64_t * restrict reduced_matrix,
                      uint64_t * restrict vector,
                      size_t n, size_t m) {
  size_t n_words = 4;

  reduced_matrix = ASSUME_ALIGNED(reduced_matrix);
  vector = ASSUME_ALIGNED(vector);

  size_t pivot = 0;
  size_t first_bit = find_first_bit(vector, n);

  while (first_bit != n) {
    size_t pivot_bit = 0;
    while (pivot < m &&
           (pivot_bit = find_first_bit(
             reduced_matrix + n_words * pivot, n)) < first_bit) {
      pivot++;
    }

    if (pivot == m)
      return false;

    add_vectors(vector, reduced_matrix + pivot * n_words, n);

    first_bit = find_first_bit(vector, n);
    pivot++;
  }

  return true;
}

static void print_matrix(uint64_t * restrict matrix, size_t n, size_t m) {
  /* size_t n_words = ((n + 63) & ~63ul) / 64; */
  size_t n_words = 4;

  for (size_t i = 0; i < m; i++) {
    for (size_t j = 0; j < n; j++) {
      if (matrix[j / 64 + n_words * i] & (1ul << (j % 64)))
        printf("1");
      else
        printf(" ");
    }
    printf("\n");
  }
}

bool check_homology(solver *solver) {
  uint32_t next_quad;
  for (next_quad = solver->quad_queue_offset;
       next_quad < solver->quad_queue_size &&
         solver->num_occurrences[solver->quad_queue[next_quad]] == 0;
       next_quad++);

  if (next_quad != solver->quad_queue_size &&
      solver->hex_id % 8 != 0)
    return true;

  uint8_t edge_id[solver->num_vertices * solver->num_vertices];
  memset(edge_id, UINT8_MAX, sizeof(edge_id));

  uint32_t num_interior = solver->num_hexes * 6 - solver->num_quads;
  uint32_t num_boundary = solver->num_quads - num_interior;

  uint32_t num_quads = 0;
  uint32_t num_edges = 0;

  for (quad_index i = 0; i < solver->max_quad_id; i++) {
    if (i >= num_boundary || solver->num_occurrences[i] != 1) {
      num_quads++;

      for (quad_vertex_index j = 0; j < 4; j++) {
        vertex_index a = solver->quads[4 * i + j];
        vertex_index b = solver->quads[4 * i + (j + 1) % 4];

        uint32_t loc = diagonal_index(solver->num_vertices, a, b);
        if (edge_id[loc] == UINT8_MAX)
          edge_id[loc] = num_edges++;
      }
    }
  }

  /* size_t num_quad_words = ((num_quads + 63) & ~63ul) / 64; */
  /* size_t num_edge_words = ((num_edges + 63) & ~63ul) / 64; */

  size_t num_quad_words = 4, num_edge_words = 4;

  ALIGNED uint64_t del2[num_edge_words * num_quads];
  memset(del2, 0, sizeof(del2));

  uint8_t quad_map[solver->max_quad_id];
  ALIGNED uint64_t finished_ids[num_quad_words];
  memset(finished_ids, 0, sizeof(finished_ids));

  uint32_t storage_i = 0;
  for (quad_index i = 0; i < solver->max_quad_id; i++) {
    if (i < num_boundary && solver->num_occurrences[i] == 1)
      continue;

    if (solver->num_occurrences[i] == 0) {
      quad_map[i] = storage_i;
      finished_ids[storage_i / 64] |= 1ul << (storage_i % 64);
    }

    for (quad_vertex_index j = 0; j < 4; j++) {
      vertex_index a = solver->quads[4 * i + j];
      vertex_index b = solver->quads[4 * i + (j + 1) % 4];

      uint8_t e = edge_id[diagonal_index(solver->num_vertices, a, b)];
      del2[(e / 64) + num_edge_words * storage_i] |= 1ul << (e % 64);
    }

    storage_i++;
  }

  ALIGNED uint64_t kernel[num_quad_words * num_quads];
  size_t ker_dim = nullspace(del2, kernel, num_edges, num_quads);

  if (ker_dim == (hex_index)(solver->hex_id + 1))
    return true;

  if (next_quad == solver->quad_queue_size)
    return false;

  if (ker_dim > solver->num_hexes)
    return false;

  for (size_t i = num_quads - ker_dim; i < num_quads; i++) {
    int parity = 0;
    for (size_t j = 0; j < num_quad_words; j++)
      parity ^= parity64(kernel[j + i * num_quad_words]);

    if (parity == 1)
      return false;
  }

  return true;

  ALIGNED uint64_t del3[num_quad_words * (solver->hex_id + 1)];
  memset(del3, 0, sizeof(del3));
  for (hex_index i = 0; i < solver->hex_id + 1; i++) {
    for (hex_facet_index j = 0; j < 6; j++) {
      vertex_index a = solver->hexes[8 * i + HEX_FACET_CONTENTS[j][0]];
      vertex_index b = solver->hexes[8 * i + HEX_FACET_CONTENTS[j][2]];

      quad_descriptor desc = solver->quad_descriptors[
        diagonal_index(solver->num_vertices, a, b)];

      quad_index q = quad_descriptor_quad_index(desc);
      if (solver->num_occurrences[q] == 0) {
        uint8_t id = quad_map[q];
        del3[(id / 64) + num_quad_words * i] |= 1ul << (id % 64);
      }
    }
  }

  ALIGNED uint64_t unused_kernel[4 * (solver->hex_id + 1)];
  nullspace(del3, unused_kernel, num_quads, solver->hex_id + 1);

  for (size_t i = num_quads - ker_dim; i < num_quads; i++) {
    bool contains_finished = false;
    for (size_t j = 0; j < num_quad_words; j++) {
      kernel[j + i * num_quad_words] &= finished_ids[j];
      contains_finished |= kernel[j + i * num_quad_words] == 0;
    }

    if (contains_finished) {
      if (!eliminate(del3, kernel + i * num_quad_words,
                     num_quads, solver->hex_id + 1)) {
        return false;
      }
    }
  }

  return true;
}
